﻿using System;
using System.Threading;
using System.Threading.Tasks;
using System.Diagnostics;
using System.Net.Http;
using System.Text;
using System.Text.Json;
using System.Linq;
using System.Runtime.InteropServices;
using FXEtherCAT.SDK;

namespace FXEtherCATControl
{
    public class FXEtherCATController : IDisposable
    {
        private readonly FXEtherCATSDK _sdk;

        // 定义常量，对应于EV1616DN-V2.2模块的PDO映射
        private const ushort SLAVE_NUMBER = 0;

        // 输出PDO (rx_pdo): 索引为 0x7010，包含16个bool类型输出
        private const ushort OUTPUT_U16_INDEX = 0x7010;

        // 输入PDO (tx_pdo): 索引为 0x6000，包含16个bool类型输入
        private const ushort INPUT_U16_INDEX = 0x6000;

        // Linux RT 相关定义
        private enum SchedPolicy
        {
            SCHED_FIFO = 1
        }
        [System.Runtime.InteropServices.StructLayout(System.Runtime.InteropServices.LayoutKind.Sequential)]
        private struct SchedParam
        {
            public int sched_priority;
        }

        [System.Runtime.InteropServices.DllImport("libc", EntryPoint = "sched_setscheduler")]
        private static extern int sched_setscheduler(int pid, int policy, ref SchedParam param);

        [System.Runtime.InteropServices.DllImport("libc", EntryPoint = "sched_get_priority_max")]
        private static extern int sched_get_priority_max(int policy);

        public FXEtherCATController()
        {
            _sdk = new FXEtherCATSDK();
        }

        public bool WaitForIOReady(CancellationToken cancellationToken = default)
        {
            Console.WriteLine("等待远程IO就绪...");
            int timeoutMs = 5000; // 5秒超时
            int elapsed = 0;

            while (!cancellationToken.IsCancellationRequested && elapsed < timeoutMs)
            {
                // 获取从站状态
                if (_sdk.GetSlaveDeviceStatus(SLAVE_NUMBER, out SlaveDeviceStatus status))
                {
                    // 检查在线状态和运行状态
                    if (status.IsOnline && status.IsOperational)
                    {
                        Console.WriteLine("远程IO已就绪");
                        return true;
                    }
                }

                Thread.Sleep(100);
                elapsed += 100;
            }

            if (elapsed >= timeoutMs)
            {
                Console.WriteLine("等待远程IO就绪超时");
                if (_sdk.GetSlaveDeviceStatus(SLAVE_NUMBER, out SlaveDeviceStatus status))
                {
                    Console.WriteLine($"在线状态: {status.IsOnline}");
                    Console.WriteLine($"运行状态: {status.IsOperational}");
                    Console.WriteLine($"AL状态: {status.AlState}");
                }
            }

            return false;
        }

        // 将字节值可视化为实心圆(●)和空心圆(○)
        private string VisualizeBytePattern(byte value)
        {
            StringBuilder sb = new StringBuilder();
            for (int i = 0; i < 8; i++) // 从低位到高位，0是右1，7是左1
            {
                bool isOn = (value & (1 << i)) != 0;
                sb.Append(isOn ? "●" : "○");
                if (i == 3) sb.Append(" "); // 每4位添加一个空格
            }
            return sb.ToString();
        }

        // 设置所有16个输出
        public void SetAllOutputs(int value)
        {
            // 在slave.json中，输出的index是0x7010，subindex从1到16
            // 每一个subindex代表一个bool类型的输出
            for (byte subindex = 1; subindex <= 16; subindex++)
            {
                _sdk.WriteSlave(SLAVE_NUMBER, OUTPUT_U16_INDEX, subindex, value);
            }
            Console.WriteLine($"所有16个数字输出已设置为: {value}");
        }

        public (int inputPattern, byte firstGroupInputs, byte secondGroupInputs) GetDigitalInputs()
        {
            // 定义非连续的子索引列表
            byte[] subindexes = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 16, 17, 18, 19, 20, 21, 22 };

            int inputPattern = 0;
            foreach (byte subindex in subindexes)
            {
                _sdk.ReadSlave(SLAVE_NUMBER, INPUT_U16_INDEX, subindex, out int value);
                if (value > 0) // 如果读取到的是非零值，说明该位为1
                {
                    inputPattern |= (1 << (subindex - 1));
                }
            }

            byte firstGroupInputs = (byte)(inputPattern & 0xFF);
            byte secondGroupInputs = (byte)((inputPattern >> 8) & 0xFF);

            // 显示输入状态
            Console.WriteLine($"读取数字输入: 0x{inputPattern:X4}");
            Console.Write("输入状态位: ");
            for (int i = 15; i >= 0; i--)
            {
                bool isOn = (inputPattern & (1 << i)) != 0;
                Console.Write(isOn ? "1" : "0");
                if (i == 8)
                    Console.Write(" ");
            }
            Console.WriteLine();

            // 可视化显示
            Console.WriteLine($"可视化状态: 第一组: {VisualizeBytePattern(firstGroupInputs)} | 第二组: {VisualizeBytePattern(secondGroupInputs)}");

            return (inputPattern, firstGroupInputs, secondGroupInputs);
        }

        public void EnterRealtimeMode()
        {
            var param = new SchedParam { sched_priority = sched_get_priority_max((int)SchedPolicy.SCHED_FIFO) };
            if (sched_setscheduler(0, (int)SchedPolicy.SCHED_FIFO, ref param) != 0)
            {
                Console.WriteLine("Warning: Failed to set SCHED_FIFO scheduler");
            }
        }

        public async Task RunBasicTest(CancellationToken cancellationToken)
        {
            try
            {
                if (!WaitForIOReady(cancellationToken))
                {
                    Console.WriteLine("IO未就绪，退出控制序列");
                    return;
                }

                // 将所有输出设置为常亮
                Console.WriteLine("设置所有输出为常亮...");
                SetAllOutputs(1);
                Console.WriteLine("所有输出已设置为常亮。");

                Console.WriteLine("进入输入监听模式，周期500ms刷新一次。按Ctrl+C退出。");

                while (!cancellationToken.IsCancellationRequested)
                {
                    GetDigitalInputs();
                    await Task.Delay(500, cancellationToken);
                }
            }
            catch (OperationCanceledException)
            {
                Console.WriteLine("控制序列被取消");
            }
            catch (Exception ex)
            {
                Console.WriteLine($"控制序列错误: {ex.Message}");
            }
            finally
            {
                SetAllOutputs(0); // 退出前关闭所有输出
                Console.WriteLine("控制序列结束");
            }
        }

        public void Dispose()
        {
            _sdk?.Dispose();
        }
    }

    class Program
    {
        // ... (Program class remains unchanged)
        // 主站信息响应模型
        private class MasterInfo
        {
            public int masterIndex { get; set; }
        }
        private class MasterInfoResponse
        {
            public int code { get; set; }
            public string msg { get; set; }
            public MasterInfo data { get; set; }
        }
        // 从站状态响应模型
        private class SlaveStatus
        {
            public int index { get; set; }
            public int master { get; set; }
            public string name { get; set; }
            public string state { get; set; }
            public string group { get; set; }
            public bool online { get; set; }
            public int operationalStatus { get; set; }
            public int alState { get; set; }
            public string vendorId { get; set; }
            public string productCode { get; set; }
            public string position { get; set; }
        }
        private class SlaveStatusResponse
        {
            public int totalCount { get; set; }
            public int opCount { get; set; }
            public int preopCount { get; set; }
            public int otherCount { get; set; }
            public List<SlaveStatus> slaves { get; set; }
        }
        private class SlaveStatusApiResponse
        {
            public int code { get; set; }
            public string msg { get; set; }
            public SlaveStatusResponse data { get; set; }
        }

        private class ShmFile
        {
            public string shmFile { get; set; }
        }
        private class ApiResponse
        {
            public int code { get; set; }
            public string msg { get; set; }
            public ShmFile data { get; set; }
        }
        private static bool IsValidHexValue(string hex)
        {
            if (string.IsNullOrEmpty(hex)) return false;
            // 移除0x前缀后检查是否全为0
            string value = hex.StartsWith("0x") ? hex.Substring(2) : hex;
            return !value.All(c => c == '0');
        }
        // 获取主站信息
        private static async Task<int> GetMasterIndex(HttpClient client, string programName)
        {
            try
            {
                Console.WriteLine("正在获取主站信息...");
                var response = await client.GetAsync(
                    $"http://127.0.0.1:3000/api/programs/master-info?programName={programName}"
                );
                if (!response.IsSuccessStatusCode)
                {
                    Console.WriteLine($"获取主站信息失败: {response.StatusCode}");
                    return -1;
                }
                var responseContent = await response.Content.ReadAsStringAsync();
                Console.WriteLine($"API返回数据: {responseContent}");
                var masterResponse = JsonSerializer.Deserialize<MasterInfoResponse>(responseContent);
                if (masterResponse.code != 0)
                {
                    Console.WriteLine($"获取主站信息API错误: {masterResponse.msg}");
                    return -1;
                }
                Console.WriteLine($"获取到主站索引: {masterResponse.data.masterIndex}");
                return masterResponse.data.masterIndex;
            }
            catch (Exception ex)
            {
                Console.WriteLine($"获取主站信息时发生错误: {ex.Message}");
                Console.WriteLine($"错误详情: {ex}");
                return -1;
            }
        }
        private static async Task<bool> CheckSlavesStatus(HttpClient client, int masterIndex)
        {
            const int MAX_RETRIES = 10;
            const int RETRY_DELAY_MS = 1000;
            for (int i = 0; i < MAX_RETRIES; i++)
            {
                try
                {
                    Console.WriteLine($"正在检查从站状态，第{i + 1}次尝试...");

                    var response = await client.GetAsync(
                        "http://127.0.0.1:3000/api/ethercat/all-slaves-status?detail=1"
                    );
                    if (!response.IsSuccessStatusCode)
                    {
                        Console.WriteLine($"获取从站状态失败: {response.StatusCode}");
                        continue;
                    }
                    var responseContent = await response.Content.ReadAsStringAsync();
                    var statusResponse = JsonSerializer.Deserialize<SlaveStatusApiResponse>(responseContent);
                    if (statusResponse.code != 0)
                    {
                        Console.WriteLine($"API错误: {statusResponse.msg}");
                        continue;
                    }
                    var data = statusResponse.data;

                    // 筛选出同主站下的从站
                    var masterSlaves = data.slaves.Where(s => s.master == masterIndex).ToList();
                    if (!masterSlaves.Any())
                    {
                        Console.WriteLine($"未找到主站 {masterIndex} 下的从站");
                        await Task.Delay(RETRY_DELAY_MS);
                        continue;
                    }
                    Console.WriteLine($"\n当前主站 {masterIndex} 下的从站状态:");
                    foreach (var slave in masterSlaves)
                    {
                        Console.WriteLine($"从站索引: {slave.index}");
                        Console.WriteLine($"名称: {slave.name}");
                        Console.WriteLine($"状态: {slave.state}");
                        Console.WriteLine($"分组: {slave.group}");
                        Console.WriteLine($"在线: {slave.online}");
                        Console.WriteLine($"运行状态: {slave.operationalStatus}");
                        Console.WriteLine($"AL状态: {slave.alState}");
                        Console.WriteLine($"位置: {slave.position}");
                        Console.WriteLine("------------------------");
                    }
                    // 检查所有从站是否都在线且状态正常
                    bool allSlavesOk = masterSlaves.All(s =>
                        s.online &&
                        (s.state == "OP" || s.state == "PREOP") &&
                        IsValidHexValue(s.vendorId) &&
                        IsValidHexValue(s.productCode)
                    );
                    if (allSlavesOk)
                    {
                        Console.WriteLine("所有从站状态正常");
                        return true;
                    }
                    Console.WriteLine("部分从站状态异常，等待重试...");
                    await Task.Delay(RETRY_DELAY_MS);
                }
                catch (Exception ex)
                {
                    Console.WriteLine($"检查从站状态时发生错误: {ex.Message}");
                    await Task.Delay(RETRY_DELAY_MS);
                }
            }
            Console.WriteLine($"从站状态检查失败，已达到最大重试次数({MAX_RETRIES})");
            return false;
        }

        private static async Task<string> StartMiddleware(string programName)
        {
            try
            {
                using var client = new HttpClient();
                client.DefaultRequestHeaders.Accept.Add(
                    new System.Net.Http.Headers.MediaTypeWithQualityHeaderValue("application/json")
                );

                // 添加sdk=true参数
                var requestData = new { programName, sdk = true };
                var jsonContent = JsonSerializer.Serialize(requestData);
                var content = new StringContent(jsonContent, Encoding.UTF8, "application/json");
                Console.WriteLine($"正在请求EtherCAT中间层服务... (SDK模式)");
                var response = await client.PostAsync(
                    "http://127.0.0.1:3000/api/programs/start-middleware",
                    content
                );
                if (!response.IsSuccessStatusCode)
                {
                    Console.WriteLine($"请求中间件失败: {response.StatusCode}");
                    return null;
                }
                var responseContent = await response.Content.ReadAsStringAsync();
                Console.WriteLine($"中间件返回数据: {responseContent}");

                var responseData = JsonSerializer.Deserialize<ApiResponse>(responseContent);
                if (responseData.code != 0)
                {
                    string errorMsg = !string.IsNullOrEmpty(responseData.msg) ? responseData.msg : "未知错误";
                    Console.WriteLine($"错误：中间件返回错误码 {responseData.code}，错误信息：{errorMsg}");
                    return null;
                }
                if (responseData.data == null || string.IsNullOrEmpty(responseData.data.shmFile))
                {
                    Console.WriteLine("错误：从中间件获取的共享内存路径为空");
                    return null;
                }

                var sharedMemoryFilePath = responseData.data.shmFile;
                Console.WriteLine($"启动成功，共享内存文件路径: {sharedMemoryFilePath}");
                return sharedMemoryFilePath;
            }
            catch (Exception ex)
            {
                Console.WriteLine($"启动中间层失败: {ex.Message}");
                return null;
            }
        }

        static async Task Main(string[] args)
        {
            try
            {
                // 创建HTTP客户端
                using var client = new HttpClient();
                client.DefaultRequestHeaders.Accept.Add(
                    new System.Net.Http.Headers.MediaTypeWithQualityHeaderValue("application/json")
                );

                string programName = Path.GetFileName(Process.GetCurrentProcess().MainModule?.FileName ?? "unknown");

                // 获取主站索引
                int masterIndex = await GetMasterIndex(client, programName);
                if (masterIndex == -1)
                {
                    Console.WriteLine("无法获取主站索引，程序退出");
                    return;
                }

                // 检查从站状态（使用主站索引）
                Console.WriteLine("开始检查从站状态...");
                if (!await CheckSlavesStatus(client, masterIndex))
                {
                    Console.WriteLine("从站状态检查失败，程序退出");
                    return;
                }

                // 启动中间层服务（SDK仍然需要中间层服务运行）
                string sharedMemoryFilePath = await StartMiddleware(programName);
                if (string.IsNullOrEmpty(sharedMemoryFilePath))
                {
                    Console.WriteLine("无法启动中间层服务，程序退出");
                    return;
                }

                Console.WriteLine($"启动FXEtherCAT控制程序...");

                using var controller = new FXEtherCATController();
                controller.EnterRealtimeMode();
                var cts = new CancellationTokenSource();
                // 添加SIGTERM信号处理
                AppDomain.CurrentDomain.ProcessExit += (s, e) => {
                    Console.WriteLine("收到SIGTERM信号，正在优雅退出...");
                    cts.Cancel();
                };
                Console.CancelKeyPress += (s, e) => {
                    e.Cancel = true;
                    cts.Cancel();
                };
                try
                {
                    await controller.RunBasicTest(cts.Token);
                }
                catch (OperationCanceledException)
                {
                    Console.WriteLine("程序正在停止...");
                }
                catch (Exception ex)
                {
                    Console.WriteLine($"执行错误: {ex.Message}");
                }
            }
            catch (Exception ex)
            {
                Console.WriteLine($"程序启动错误: {ex.Message}");
            }
        }
    }
}